<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN"
                "http://www.w3.org/TR/REC-html40/loose.dtd">
<html>
<head>
  <title>Description of srcdkf</title>
  <meta name="keywords" content="srcdkf">
  <meta name="description" content="SRCDKF  Square Root Central Difference Kalman Filter (Sigma-Point Kalman Filter variant)">
  <meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1">
  <meta name="generator" content="m2html &copy; 2003 Guillaume Flandin">
  <meta name="robots" content="index, follow">
  <link type="text/css" rel="stylesheet" href="../../m2html.css">
</head>
<body>
<a name="_top"></a>
<div><a href="../../menu.html">Home</a> &gt;  <a href="#">ReBEL-0.2.7</a> &gt; <a href="#">core</a> &gt; srcdkf.m</div>

<!--<table width="100%"><tr><td align="left"><a href="../../menu.html"><img alt="<" border="0" src="../../left.png">&nbsp;Master index</a></td>
<td align="right"><a href="menu.html">Index for .\ReBEL-0.2.7\core&nbsp;<img alt=">" border="0" src="../../right.png"></a></td></tr></table>-->

<h1>srcdkf
</h1>

<h2><a name="_name"></a>PURPOSE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>SRCDKF  Square Root Central Difference Kalman Filter (Sigma-Point Kalman Filter variant)</strong></div>

<h2><a name="_synopsis"></a>SYNOPSIS <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srcdkf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS) </strong></div>

<h2><a name="_description"></a>DESCRIPTION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre class="comment"> SRCDKF  Square Root Central Difference Kalman Filter (Sigma-Point Kalman Filter variant)

   [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srcdkf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)

   This filter assumes the following standard state-space model:

     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
     y(k) = hfun[x(k),n(k),U2(k)]

   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state
   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the
   system.

   INPUT
         state                state mean at time k-1          ( xh(k-1) )
         Sstate               lower triangular Cholesky factor of state covariance at time k-1    ( Sx(k-1) )
         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')
         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')
         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
         InferenceDS          SPOK inference data structure generated by GENINFDS function.

   OUTPUT
         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )
         Sx                   Cholesky factor of state covariance at time k  ( Sx(k) )
         pNoise               process noise data structure     (possibly updated)
         oNoise               observation noise data structure (possibly updated)

         InternalVariablesDS  (optional) internal variables data structure
            .xh_                 predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )
            .Sx_                 predicted state covariance (Cholesky factor)
            .yh_                 predicted observation ( E[y(k)|Y(k-1)] )
            .inov                inovation signal
            .Pinov               inovation covariance
            .KG                  Kalman gain

   Required InferenceDS fields:
         .spkfParams           SPKF parameters = [h] with
                                    h  :  CDKF scale factor / difference step size

   See also
   <a href="kf.html" class="code" title="function [xh, Px, pNoise, oNoise, InternalVariablesDS] = kf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">KF</a>, <a href="ekf.html" class="code" title="function [xh, Px, pNoise, oNoise, InternalVariablesDS] = ekf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">EKF</a>, <a href="ukf.html" class="code" title="function [xh, Px, pNoise, oNoise, InternalVariablesDS] = ukf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">UKF</a>, <a href="cdkf.html" class="code" title="function [xh, Px, pNoise, oNoise, InternalVariablesDS] = cdkf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">CDKF</a>, <a href="srukf.html" class="code" title="function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srukf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)">SRUKF</a>
   Copyright (c) Oregon Health &amp; Science University (2006)

   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
   academic use only (see included license file) and can be obtained from
   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
   software should contact rebel@csee.ogi.edu for commercial licensing information.

   See LICENSE (which should be part of the main toolkit distribution) for more
   detail.</pre></div>

<!-- crossreference -->
<h2><a name="_cross"></a>CROSS-REFERENCE INFORMATION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
This function calls:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>	ADDANGLE   Addition function for 'angle space' sigma-points expressed in radians.</li><li><a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>	CVECREP  Column vector replicate</li><li><a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>	ADDANGLE   Addition function for 'angle space' sigma-points expressed in radians.</li></ul>
This function is called by:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="gmsppf.html" class="code" title="function [estimate, ParticleFilterDS, pNoise, oNoise, extra] = gmsppf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)">gmsppf</a>	GMSPPF  Gaussian Mixture Sigma-Point Particle Filter</li><li><a href="sppf.html" class="code" title="function [estimate, ParticleFilterDS, pNoise, oNoise] = sppf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)">sppf</a>	SPPF  Sigma-Point Particle Filter.</li><li><a href="../.././ReBEL-0.2.7/examples/dual_estimation/demspeech_dual.html" class="code" title="">demspeech_dual</a>	DEMSPEECH_DUAL  Sigma-Point Kalman Filter based Speech Enhancement Demonstration.</li><li><a href="../.././ReBEL-0.2.7/examples/joint_estimation/demje2.html" class="code" title="">demje2</a>	DEMJE2 Demonstrate nonlinear time series joint estimation for Mackey-Glass chaotic time series</li><li><a href="../.././ReBEL-0.2.7/examples/parameter_estimation/dempe1.html" class="code" title="">dempe1</a>	DEMPE1  Demonstrate parameter estimation on a simple 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/parameter_estimation/dempe2.html" class="code" title="">dempe2</a>	DEMPE2  Demonstrate how the ReBEL toolkit is used to train a neural network</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse1.html" class="code" title="">demse1</a>	DEMSE1  Demonstrate state estimation on a simple 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse2.html" class="code" title="">demse2</a>	DEMSE2  Demonstrate state estimation on a simple scalar nonlinear (time variant) problem</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse3.html" class="code" title="">demse3</a>	DEMSE3  Demonstrate nonlinear time series state estimation for Mackey-Glass chaotic time series</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse4.html" class="code" title="">demse4</a>	DEMSE4  Bearing Only Tracking Example</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse5.html" class="code" title="">demse5</a>	DEMSE4  Bearing and Frequency Tracking Example</li></ul>
<!-- crossreference -->


<h2><a name="_source"></a>SOURCE CODE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre>0001 <a name="_sub0" href="#_subfunctions" class="code">function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srcdkf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</a>
0002 
0003 <span class="comment">% SRCDKF  Square Root Central Difference Kalman Filter (Sigma-Point Kalman Filter variant)</span>
0004 <span class="comment">%</span>
0005 <span class="comment">%   [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srcdkf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</span>
0006 <span class="comment">%</span>
0007 <span class="comment">%   This filter assumes the following standard state-space model:</span>
0008 <span class="comment">%</span>
0009 <span class="comment">%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]</span>
0010 <span class="comment">%     y(k) = hfun[x(k),n(k),U2(k)]</span>
0011 <span class="comment">%</span>
0012 <span class="comment">%   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state</span>
0013 <span class="comment">%   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the</span>
0014 <span class="comment">%   system.</span>
0015 <span class="comment">%</span>
0016 <span class="comment">%   INPUT</span>
0017 <span class="comment">%         state                state mean at time k-1          ( xh(k-1) )</span>
0018 <span class="comment">%         Sstate               lower triangular Cholesky factor of state covariance at time k-1    ( Sx(k-1) )</span>
0019 <span class="comment">%         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')</span>
0020 <span class="comment">%         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')</span>
0021 <span class="comment">%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )</span>
0022 <span class="comment">%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )</span>
0023 <span class="comment">%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )</span>
0024 <span class="comment">%         InferenceDS          SPOK inference data structure generated by GENINFDS function.</span>
0025 <span class="comment">%</span>
0026 <span class="comment">%   OUTPUT</span>
0027 <span class="comment">%         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )</span>
0028 <span class="comment">%         Sx                   Cholesky factor of state covariance at time k  ( Sx(k) )</span>
0029 <span class="comment">%         pNoise               process noise data structure     (possibly updated)</span>
0030 <span class="comment">%         oNoise               observation noise data structure (possibly updated)</span>
0031 <span class="comment">%</span>
0032 <span class="comment">%         InternalVariablesDS  (optional) internal variables data structure</span>
0033 <span class="comment">%            .xh_                 predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )</span>
0034 <span class="comment">%            .Sx_                 predicted state covariance (Cholesky factor)</span>
0035 <span class="comment">%            .yh_                 predicted observation ( E[y(k)|Y(k-1)] )</span>
0036 <span class="comment">%            .inov                inovation signal</span>
0037 <span class="comment">%            .Pinov               inovation covariance</span>
0038 <span class="comment">%            .KG                  Kalman gain</span>
0039 <span class="comment">%</span>
0040 <span class="comment">%   Required InferenceDS fields:</span>
0041 <span class="comment">%         .spkfParams           SPKF parameters = [h] with</span>
0042 <span class="comment">%                                    h  :  CDKF scale factor / difference step size</span>
0043 <span class="comment">%</span>
0044 <span class="comment">%   See also</span>
0045 <span class="comment">%   KF, EKF, UKF, CDKF, SRUKF</span>
0046 <span class="comment">%   Copyright (c) Oregon Health &amp; Science University (2006)</span>
0047 <span class="comment">%</span>
0048 <span class="comment">%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for</span>
0049 <span class="comment">%   academic use only (see included license file) and can be obtained from</span>
0050 <span class="comment">%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the</span>
0051 <span class="comment">%   software should contact rebel@csee.ogi.edu for commercial licensing information.</span>
0052 <span class="comment">%</span>
0053 <span class="comment">%   See LICENSE (which should be part of the main toolkit distribution) for more</span>
0054 <span class="comment">%   detail.</span>
0055 
0056 <span class="comment">%=============================================================================================</span>
0057 
0058 Xdim  = InferenceDS.statedim;                                <span class="comment">% extract state dimension</span>
0059 Odim  = InferenceDS.obsdim;                                  <span class="comment">% extract observation dimension</span>
0060 U1dim = InferenceDS.U1dim;                                   <span class="comment">% extract exogenous input 1 dimension</span>
0061 U2dim = InferenceDS.U2dim;                                   <span class="comment">% extract exogenous input 2 dimension</span>
0062 Vdim  = InferenceDS.Vdim;                                    <span class="comment">% extract process noise dimension</span>
0063 Ndim  = InferenceDS.Ndim;                                    <span class="comment">% extract observation noise dimension</span>
0064 
0065 NOV = size(obs,2);                                           <span class="comment">% number of input vectors</span>
0066 
0067 <span class="comment">%------------------------------------------------------------------------------------------------------------------</span>
0068 <span class="comment">%-- ERROR CHECKING</span>
0069 
0070 <span class="keyword">if</span> (nargin ~= 8) error(<span class="string">' [ srcdkf ] Not enough input arguments.'</span>); <span class="keyword">end</span>
0071 
0072 <span class="keyword">if</span> (Xdim~=size(state,1)) error(<span class="string">'[ srcdkf ] Prior state dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0073 <span class="keyword">if</span> (Xdim~=size(Sstate,1)) error(<span class="string">'[ srcdkf ] Prior state covariance dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0074 <span class="keyword">if</span> (Odim~=size(obs,1)) error(<span class="string">'[ srcdkf ] Observation dimension differs from InferenceDS.obsdim'</span>); <span class="keyword">end</span>
0075 <span class="keyword">if</span> U1dim
0076   [dim,nop] = size(U1);
0077   <span class="keyword">if</span> (U1dim~=dim) error(<span class="string">'[ srcdkf ] Exogenous input U1 dimension differs from InferenceDS.U1dim'</span>); <span class="keyword">end</span>
0078   <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">'[ srcdkf ] Number of observation vectors and number of exogenous input U1 vectors do not agree!'</span>); <span class="keyword">end</span>
0079 <span class="keyword">end</span>
0080 <span class="keyword">if</span> U2dim
0081   [dim,nop] = size(U2);
0082   <span class="keyword">if</span> (U2dim~=dim) error(<span class="string">'[ srcdkf ] Exogenous input U2 dimension differs from InferenceDS.U2dim'</span>); <span class="keyword">end</span>
0083   <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">'[ srcdkf ] Number of observation vectors and number of exogenous input U2 vectors do not agree!'</span>); <span class="keyword">end</span>
0084 <span class="keyword">end</span>
0085 
0086 <span class="comment">%------------------------------------------------------------------------------------------------------------------------</span>
0087 
0088 <span class="comment">% setup buffer</span>
0089 xh   = zeros(Xdim,NOV);
0090 xh_  = zeros(Xdim,NOV);
0091 yh_  = zeros(Odim,NOV);
0092 inov = zeros(Odim,NOV);
0093 
0094 <span class="comment">% Get index vectors for any of the state or observation vector components that are angular quantities</span>
0095 <span class="comment">% which have discontinuities at +- Pi radians ?</span>
0096 
0097 sA_IdxVec = InferenceDS.stateAngleCompIdxVec;
0098 oA_IdxVec = InferenceDS.obsAngleCompIdxVec;
0099 
0100 
0101 <span class="comment">% Get and calculate CDKF scaling parameters and sigma point weights</span>
0102 h = InferenceDS.spkfParams(1);
0103 hh = h^2;
0104 
0105 W1 = [(hh - Xdim - Vdim)/hh   1/(2*hh);                  <span class="comment">% sigma-point weights set 1</span>
0106       1/(2*h)                sqrt(hh-1)/(2*hh)];
0107 
0108 W2      = W1;
0109 W2(1,1) = (hh - Xdim - Ndim)/hh ;                        <span class="comment">% sigma-point weights set 2</span>
0110 
0111 
0112 
0113 <span class="keyword">switch</span> InferenceDS.inftype
0114 
0115 <span class="comment">%======================================= PARAMETER ESTIMATION VERSION ===========================================</span>
0116 <span class="keyword">case</span> <span class="string">'parameter'</span>
0117 
0118     Zeros_Xdim_X_Ndim     = zeros(Xdim,Ndim);
0119     Zeros_Ndim_X_Xdim     = zeros(Ndim,Xdim);
0120 
0121     nsp2 = 2*(Xdim+Ndim) + 1;                                <span class="comment">% number of sigma points</span>
0122 
0123     Sv = pNoise.cov;                             <span class="comment">% Cholesky factor of process noise covariance</span>
0124     dv = diag(Sv);
0125     Sn = oNoise.cov;                             <span class="comment">% Cholesky factor of measurement noise covariance</span>
0126     mu_n = oNoise.mu;
0127 
0128     Sx = Sstate;
0129 
0130     <span class="comment">%---  Loop over all input vectors ---</span>
0131     <span class="keyword">for</span> i=1:NOV,
0132 
0133         UU2 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U2(:,i),nsp2);
0134 
0135         <span class="comment">%------------------------------------------------------</span>
0136         <span class="comment">% TIME UPDATE</span>
0137 
0138         xh_(:,i) = state;
0139 
0140         <span class="keyword">if</span> pNoise.adaptMethod
0141         <span class="comment">%--------------------------------------------</span>
0142             <span class="keyword">switch</span> pNoise.adaptMethod
0143 
0144             <span class="keyword">case</span> <span class="string">'lambda-decay'</span>
0145                 Sx_ = sqrt(pNoise.adaptParams(1))*Sx;
0146 
0147             <span class="keyword">case</span> {<span class="string">'anneal'</span>,<span class="string">'robbins-monro'</span>}
0148                 Sx_ = Sx + Sv;
0149 
0150             <span class="keyword">end</span>
0151         <span class="comment">%---------------------------------------------</span>
0152         <span class="keyword">else</span>
0153             Sx_ = Sx;
0154         <span class="keyword">end</span>
0155 
0156         Z   = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>([xh_(:,i); mu_n],nsp2);
0157         Sz  = [Sx_ Zeros_Xdim_X_Ndim; Zeros_Ndim_X_Xdim Sn];
0158         hSz  = h*Sz;
0159         Z(:,2:nsp2) = Z(:,2:nsp2) + [hSz -hSz];
0160 
0161         Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);
0162 
0163         <span class="comment">%-- Calculate predicted observation mean, dealing with angular discontinuities if needed</span>
0164         <span class="keyword">if</span> isempty(oA_IdxVec)
0165             yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);           <span class="comment">% pediction of observation</span>
0166             C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
0167             D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(2*Y_(:,1),Xdim+Ndim));
0168         <span class="keyword">else</span>
0169             obs_pivotA = Y_(oA_IdxVec,1);      <span class="comment">% extract pivot angle</span>
0170             Y_(oA_IdxVec,1) = 0;
0171             Y_(oA_IdxVec,2:nsp2) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(Y_(oA_IdxVec,2:nsp2),<a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(obs_pivotA,nsp2-1));  <span class="comment">% subtract pivot angle mod 2pi</span>
0172             yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);           <span class="comment">% pediction of observation</span>
0173             yh_(oA_IdxVec,i) = 0;
0174             <span class="keyword">for</span> k=2:nsp2,
0175                yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), W2(1,2)*Y_(oA_IdxVec,k));   <span class="comment">% calculate CDKF mean ... mod 2pi</span>
0176             <span class="keyword">end</span>
0177             C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
0178             D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(2*Y_(:,1),Xdim+Ndim));
0179             C(oA_IdxVec,:) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(W2(2,1)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,1)*Y_(oA_IdxVec, Xdim+Ndim+2:nsp2));
0180             D(oA_IdxVec,:) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(W2(2,2)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,2)*Y_(oA_IdxVec,Xdim+Ndim+2:nsp2));
0181             <span class="comment">% Note for above line : Remember, Y_(oA_IdxVec,1) = 0, so the last term of D expression need not be subtracted</span>
0182             yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), obs_pivotA);  <span class="comment">% add pivot angle back to calculate actual predicted mean</span>
0183         <span class="keyword">end</span>
0184 
0185         [temp,Sy] = qr([C D]',0);             <span class="comment">% calculate inovation covariance using QR</span>
0186         Sy = Sy';                             <span class="comment">% convert back to ReBEL format</span>
0187 
0188         <span class="keyword">if</span> (Sy==0), Sy = eps; <span class="keyword">end</span>
0189 
0190         Syx1 = C(:,1:Xdim);
0191         Syw1 = C(:,Xdim+1:end);
0192 
0193         Pxy = Sx_*Syx1';
0194 
0195         KG = (Pxy / Sy') / Sy;                <span class="comment">% Kalman gain using LS solution with pivot</span>
0196 
0197 
0198         <span class="keyword">if</span> isempty(InferenceDS.innovation)
0199             inov(:,i) = obs(:,i) - yh_(:,i);
0200         <span class="keyword">else</span>
0201             inov(:,i) = InferenceDS.innovation( InferenceDS, obs(:,i), yh_(:,i));  <span class="comment">% inovation (observation error)</span>
0202         <span class="keyword">end</span>
0203 
0204 
0205         <span class="keyword">if</span> isempty(sA_IdxVec)
0206            xh(:,i) = xh_(:,i) + KG*inov(:,i);
0207         <span class="keyword">else</span>
0208            upd = KG*inov(:,i);
0209            xh(:,i) = xh_(:,i) + upd;
0210            xh(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), upd(sA_IdxVec));
0211         <span class="keyword">end</span>
0212 
0213 
0214 
0215         state = xh(:,i);
0216 
0217         Sx_ = Sx_';                           <span class="comment">% ReBEL form -&gt; Matlab form</span>
0218 
0219         cov_update_vectors = KG*Sy;           <span class="comment">% Correct covariance. This is equivalent to :  Px = Px_ - KG*Py*KG';</span>
0220         <span class="keyword">for</span> j=1:Odim
0221             Sx_ = cholupdate(Sx_,cov_update_vectors(:,j),<span class="string">'-'</span>);
0222         <span class="keyword">end</span>
0223 
0224         Sx = Sx_';                            <span class="comment">% Matlab form -&gt; ReBEL form</span>
0225         state = xh(:,i);
0226 
0227         <span class="keyword">if</span> pNoise.adaptMethod
0228         <span class="comment">%--- update process noise if needed -----------------------</span>
0229             <span class="keyword">switch</span> pNoise.adaptMethod
0230 
0231             <span class="keyword">case</span> <span class="string">'anneal'</span>
0232                 dV = max(pNoise.adaptParams(1)*(dv.^2) , pNoise.adaptParams(2));
0233                 ds = diag(Sx);
0234                 dv = -ds + sqrt(dV + ds.^2);
0235                 Sv = diag(dv);
0236 
0237             <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0238                 nu = 1/pNoise.adaptParams(1);
0239                 dV = (1-nu)*(dv.^2) + nu*diag(KG*(KG*inov*inov')');
0240                 ds = diag(Sx);
0241                 dv = -ds + sqrt(dV + ds.^2);
0242                 Sv = diag(dv);
0243                 pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0244 
0245             <span class="keyword">otherwise</span>
0246                 error(<span class="string">' [ srukf ]  Process noise update method not allowed.'</span>);
0247 
0248             <span class="keyword">end</span>
0249 
0250             pNoise.cov = Sv;
0251         <span class="comment">%-----------------------------------------------------------</span>
0252         <span class="keyword">end</span>
0253 
0254     <span class="keyword">end</span>   <span class="comment">%... loop over all input vectors</span>
0255 
0256 
0257 
0258 <span class="keyword">otherwise</span>
0259 <span class="comment">%===================================== STATE &amp; JOINT ESTIMATION VERSION =========================================</span>
0260 
0261     Zeros_Xdim_X_Vdim = zeros(Xdim,Vdim);
0262     Zeros_Vdim_X_Xdim = zeros(Vdim,Xdim);
0263     Zeros_Xdim_X_Ndim = zeros(Xdim,Ndim);
0264     Zeros_Ndim_X_Xdim = zeros(Ndim,Xdim);
0265 
0266     nsp1   = 2*(Xdim+Vdim) + 1;          <span class="comment">% number of sigma points (first set)</span>
0267     nsp2   = 2*(Xdim+Ndim) + 1;          <span class="comment">% number of sigma points (second set)</span>
0268 
0269     Sv = pNoise.cov;         <span class="comment">% matrix square root of process noise covariance</span>
0270     Sn = oNoise.cov;         <span class="comment">% matrix square root of measurement noise covariance</span>
0271     mu_v = pNoise.mu;      <span class="comment">% get process noise mean</span>
0272     mu_n = oNoise.mu;      <span class="comment">% get measurement noise mean</span>
0273     Sx = Sstate;                         <span class="comment">% matrix square root of state covariance</span>
0274 
0275     <span class="keyword">if</span> (U1dim==0), UU1 = zeros(0,nsp1); <span class="keyword">end</span>
0276     <span class="keyword">if</span> (U2dim==0), UU2 = zeros(0,nsp2); <span class="keyword">end</span>
0277 
0278     <span class="comment">% if process noise adaptation for joint estimation</span>
0279     <span class="keyword">if</span> pNoise.adaptMethod
0280         <span class="keyword">switch</span> InferenceDS.inftype
0281           <span class="keyword">case</span> <span class="string">'joint'</span>
0282             idx = pNoise.idxArr(<span class="keyword">end</span>,:);     <span class="comment">% get indeces of parameter block of combo-gaussian noise source</span>
0283             ind1 = idx(1);                  <span class="comment">% beginning index of parameter section</span>
0284             ind2 = idx(2);                  <span class="comment">% ending index of parameter section</span>
0285             paramdim = ind2-ind1+1;         <span class="comment">% infer parameter vector length</span>
0286             dv = diag(Sv);                  <span class="comment">% grab diagonal</span>
0287             dv = dv(ind1:ind2);             <span class="comment">% extract the part of the diagonal that relates to the 'parameter section'</span>
0288           <span class="keyword">case</span> <span class="string">'state'</span>
0289             ind1 = 1;
0290             ind2 = Xdim;
0291             paramdim = Xdim;
0292             dv = diag(Sv);
0293         <span class="keyword">end</span>
0294     <span class="keyword">end</span>
0295 
0296     <span class="comment">%--- Loop over all input vectors ---</span>
0297     <span class="keyword">for</span> i=1:NOV,
0298 
0299         <span class="keyword">if</span> (U1dim), UU1 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U1(:,i),nsp1); <span class="keyword">end</span>
0300         <span class="keyword">if</span> (U2dim), UU2 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U2(:,i),nsp2); <span class="keyword">end</span>
0301 
0302         <span class="comment">%------------------------------------------------------</span>
0303         <span class="comment">% TIME UPDATE</span>
0304 
0305         Z   = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>([state; pNoise.mu],nsp1);
0306         Zm  = Z;                                                <span class="comment">% copy needed for possible angle components section</span>
0307         Sz  = [Sx Zeros_Xdim_X_Vdim; Zeros_Vdim_X_Xdim Sv];
0308         hSz = h*Sz;
0309         hSzM = [hSz -hSz];
0310         Z(:,2:nsp1) = Z(:,2:nsp1) + hSzM;         <span class="comment">% build sigma-point set</span>
0311 
0312         <span class="comment">%-- Calculate predicted state mean, dealing with angular discontinuities if needed</span>
0313         <span class="keyword">if</span> isempty(sA_IdxVec)
0314             X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1);  <span class="comment">% propagate sigma-points through process model</span>
0315             xh_(:,i) = W1(1,1)*X_(:,1) + W1(1,2)*sum(X_(:,2:nsp1),2);
0316             A = W1(2,1) * ( X_(:,2:Xdim+Vdim+1) - X_(:,Xdim+Vdim+2:nsp1) ) ;
0317             B = W1(2,2) * ( X_(:,2:Xdim+Vdim+1) + X_(:,Xdim+Vdim+2:nsp1) - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(2*X_(:,1),Xdim+Vdim));
0318         <span class="keyword">else</span>
0319             Z(sA_IdxVec,2:nsp1) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(Zm(sA_IdxVec,2:nsp1), hSzM(sA_IdxVec,:));      <span class="comment">% fix sigma-point set for angular components</span>
0320             X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1); <span class="comment">% propagate sigma-points through process model</span>
0321             state_pivotA = X_(sA_IdxVec,1);                                <span class="comment">% extract pivot angle</span>
0322             X_(sA_IdxVec,1) = 0;
0323             X_(sA_IdxVec,2:end) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(X_(sA_IdxVec,2:end), <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(state_pivotA,nsp1-1));  <span class="comment">% subtract pivot angle mod 2pi</span>
0324             xh_(:,i) = W1(1,1)*X_(:,1) + W1(1,2)*sum(X_(:,2:nsp1),2);
0325             xh_(sA_IdxVec,i) = 0;
0326             <span class="keyword">for</span> k=2:nsp1,
0327                 xh_(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), W1(1,2)*X_(sA_IdxVec,k));     <span class="comment">% calculate CDKF mean ... mod 2pi</span>
0328             <span class="keyword">end</span>
0329             A = W1(2,1) * ( X_(:,2:Xdim+Vdim+1) - X_(:,Xdim+Vdim+2:nsp1) ) ;
0330             B = W1(2,2) * ( X_(:,2:Xdim+Vdim+1) + X_(:,Xdim+Vdim+2:nsp1) - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(2*X_(:,1),Xdim+Vdim));
0331             A(sA_IdxVec,:) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(W1(2,1)*X_(sA_IdxVec,2:Xdim+Vdim+1), W1(2,1)*X_(sA_IdxVec,Xdim+Vdim+2:nsp1));
0332             B(sA_IdxVec,:) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(W1(2,2)*X_(sA_IdxVec,2:Xdim+Vdim+1), W1(2,2)*X_(sA_IdxVec,Xdim+Vdim+2:nsp1));
0333             <span class="comment">% Note for above line : Remember, X_(sA_IdxVec,1) = 0, so the last term of B expression need not be subtracted</span>
0334             xh_(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), state_pivotA);  <span class="comment">% add pivot angle back to calculate actual predicted mean</span>
0335         <span class="keyword">end</span>
0336 
0337         [temp,Sx_] = qr([A B]',0);
0338         Sx_= Sx_';
0339 
0340 
0341         Z  = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>([xh_(:,i); oNoise.mu] ,nsp2);
0342         Zm = Z;                                                 <span class="comment">% copy needed for possible angle components section</span>
0343         Sz = [Sx_ Zeros_Xdim_X_Ndim; Zeros_Ndim_X_Xdim Sn];
0344         hSz = h*Sz;
0345         hSzM = [hSz -hSz];
0346         Z(:,2:nsp2) = Z(:,2:nsp2) + hSzM;
0347 
0348         <span class="comment">%-- Calculate predicted observation mean, dealing with angular discontinuities if needed</span>
0349         <span class="keyword">if</span> isempty(oA_IdxVec)
0350             Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);
0351             yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);
0352             C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
0353             D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(2*Y_(:,1),Xdim+Ndim));
0354         <span class="keyword">else</span>
0355             Z(oA_IdxVec,2:nsp2) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(Zm(oA_IdxVec,2:nsp2), hSzM(oA_IdxVec,:));  <span class="comment">% fix sigma-point set for angular components</span>
0356             Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);
0357             obs_pivotA = Y_(oA_IdxVec,1);      <span class="comment">% extract pivot angle</span>
0358             Y_(oA_IdxVec,1) = 0;
0359             Y_(oA_IdxVec,2:nsp2) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(Y_(oA_IdxVec,2:nsp2),<a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(obs_pivotA,nsp2-1));  <span class="comment">% subtract pivot angle mod 2pi</span>
0360             yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);           <span class="comment">% pediction of observation</span>
0361             yh_(oA_IdxVec,i) = 0;
0362             <span class="keyword">for</span> k=2:nsp2,
0363                yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), W2(1,2)*Y_(oA_IdxVec,k));   <span class="comment">% calculate CDKF mean ... mod 2pi</span>
0364             <span class="keyword">end</span>
0365             C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
0366             D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(2*Y_(:,1),Xdim+Ndim));
0367             C(oA_IdxVec,:) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(W2(2,1)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,1)*Y_(oA_IdxVec, Xdim+Ndim+2:nsp2));
0368             D(oA_IdxVec,:) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(W2(2,2)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,2)*Y_(oA_IdxVec,Xdim+Ndim+2:nsp2));
0369             <span class="comment">% Note for above line : Remember, Y_(oA_IdxVec,1) = 0, so the last term of D expression need not be subtracted</span>
0370             yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), obs_pivotA);  <span class="comment">% add pivot angle back to calculate actual predicted mean</span>
0371         <span class="keyword">end</span>
0372 
0373         [temp,Sy] = qr([C D]',0);
0374         Sy = Sy';
0375 
0376 
0377         <span class="comment">%------------------------------------------------------</span>
0378         <span class="comment">% MEASUREMENT UPDATE</span>
0379 
0380         Syx1 = C(:,1:Xdim);
0381         Syw1 = C(:,Xdim+1:end);
0382 
0383         Pxy = Sx_*Syx1';
0384 
0385         KG = (Pxy / Sy') / Sy;
0386 
0387         <span class="keyword">if</span> isempty(InferenceDS.innovation)
0388             inov(:,i) = obs(:,i) - yh_(:,i);
0389             <span class="keyword">if</span> ~isempty(oA_IdxVec)
0390               inov(oA_IdxVec,i) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(obs(oA_IdxVec,i), yh_(oA_IdxVec,i));
0391             <span class="keyword">end</span>
0392         <span class="keyword">else</span>
0393             inov(:,i) = InferenceDS.innovation( InferenceDS, obs(:,i), yh_(:,i));  <span class="comment">% inovation (observation error)</span>
0394         <span class="keyword">end</span>
0395 
0396         <span class="keyword">if</span> isempty(sA_IdxVec)
0397            xh(:,i) = xh_(:,i) + KG*inov(:,i);
0398         <span class="keyword">else</span>
0399            upd = KG*inov(:,i);
0400            xh(:,i) = xh_(:,i) + upd;
0401            xh(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), upd(sA_IdxVec));
0402         <span class="keyword">end</span>
0403 
0404         state = xh(:,i);
0405 
0406         [temp,Sx] = qr([Sx_-KG*Syx1 KG*Syw1 KG*D]',0);
0407         Sx=Sx';
0408 
0409 
0410         <span class="keyword">if</span> pNoise.adaptMethod
0411         <span class="comment">%--- update process noise if needed for joint estimation ----------------------</span>
0412             <span class="keyword">switch</span> pNoise.adaptMethod
0413 
0414             <span class="keyword">case</span> <span class="string">'anneal'</span>
0415                 dv = sqrt(max(pNoise.adaptParams(1)*(dv.^2) , pNoise.adaptParams(2)));
0416                 Sv(ind1:ind2,ind1:ind2) = diag(dv);
0417 
0418              <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0419                 nu = 1/pNoise.adaptParams(1);
0420                 subKG = KG(end-paramdim+1:<span class="keyword">end</span>,:);
0421                 dv = sqrt((1-nu)*(dv.^2) + nu*diag(subKG*(subKG*inov*inov')'));
0422                 Sv(ind1:ind2,ind1:ind2) = diag(dv);
0423                 pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0424 
0425             <span class="keyword">otherwise</span>
0426                 error(<span class="string">' [ srcdkf ]  Process noise update method not allowed.'</span>);
0427 
0428             <span class="keyword">end</span>
0429 
0430             pNoise.cov = Sv;
0431 
0432 
0433         <span class="comment">%-----------------------------------------------------------</span>
0434         <span class="keyword">end</span>
0435 
0436     <span class="keyword">end</span>   <span class="comment">%--- loop over all input vectors</span>
0437 
0438 <span class="comment">%====================================================================================================================</span>
0439 <span class="keyword">end</span>
0440 
0441 
0442 <span class="keyword">if</span> (nargout&gt;4),
0443     InternalVariablesDS.xh_   = xh_;
0444     InternalVariablesDS.Sx_   = Sx_;
0445     InternalVariablesDS.yh_   = yh_;
0446     InternalVariablesDS.inov  = inov;
0447     InternalVariablesDS.Sinov = Sy;
0448     InternalVariablesDS.KG    = KG;
0449 <span class="keyword">end</span>
0450 
0451 
0452 
0453</pre></div>
<hr><address>Generated on Tue 26-Sep-2006 10:36:21 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> &copy; 2003</address>
</body>
</html>